/**
 * @file ROS_CAN_EMUC.cpp
 * @author circleup (circleup@foxmail.com)
 * @brief EMUC CAN 发送节点
 * @version 2.8
 * @date 2020-07-08
 * 
 * @copyright Copyright (c) 2020
 * 
 */


#include "lib_emuc.h"

#include "ros/ros.h"
#include "ros_car/VCU_frame.h"

#define ttyACM0 24

/**
 * @brief 设置一帧数据
 * 
 * @param data 数据指针
 */
void set_emuc_data(DATA_INFO * data);


void SendROSDatatoCAN(const ros_car::VCU_frame::ConstPtr& msg);

/**
 * @brief 工控机 CAN 数据发送节点
 * 
 * @param argc 
 * @param argv 
 * @return int 
 */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "ROS_EMUC");

  ROS_INFO("This is ROS_EMUC");

  ros::NodeHandle CAN;
  ros::Subscriber receive = CAN.subscribe("CAN_send_data", 1000, SendROSDatatoCAN);
  // ros::Publisher send = CAN.advertise<ros_car::VCU_frame>("CAN_receive_data", 1000);
  
  // ros::Rate loop_rate(50);

  DATA_INFO data;


  if (!EMUCOpenDevice(ttyACM0))
  {
    EMUCSetCAN(ttyACM0, EMUC_CH_1, EMUC_BAUDRATE_500K);
  }
  else
  {
    return 0;
  }
  
  ros::spin();
  return 0;
}

void SendROSDatatoCAN(const ros_car::VCU_frame::ConstPtr& msg)
{
  DATA_INFO tmp;
  tmp.com_port = ttyACM0;
  tmp.channel = EMUC_CH_1;  // CAN 0
  tmp.mod = 0;              // 标准帧
  tmp.rtr = 0;              // 数据帧
  tmp.dlc = 8;              // 数据长度 8

  tmp.id[0] =0x00;
  tmp.id[1] =0x00;
  tmp.id[2] =0x02;
  tmp.id[3] =0x38;

  for(int i = 0; i < 8; ++ i)
  {
    tmp.data[i] = msg->arryData[i];
  }
  EMUCSend(&tmp);
}